
import threading
import time
import modbus_tk 
import modbus_tk.defines as cst
import modbus_tk.modbus as modbus
import modbus_tk.modbus_tcp as modbus_tcp
#from haier_plc import *
from get_config import *
#D2120为偏差数值，为当前物料位置相对模板向前（-）或向后偏（+）移量，32位浮点数。

#melsec = modbus_tcp.TcpMaster(host=IP, port=PORT)
melsec = None
def modbus_tcp_reopen():
    global melsec
    if melsec is not None:
        melsec.close()
    plc_ip = config['plc_ip']
    plc_port = int(config['plc_port'])
    print(f'modbus_tcp host={plc_ip} port={plc_port}')
    melsec = modbus_tcp.TcpMaster(host=plc_ip, port=plc_port)
    melsec.set_timeout(5.0)
    return melsec

modbus_tcp_reopen()

if 0:
    while 1:
        try:
            salve_value = melsec.execute(1, cst.READ_HOLDING_REGISTERS, 611, 1)
        except Exception as e:
            print(e)


base_addr = 0

def _try_plc_read(addr=0, bit_len=1):
    try:
        if isinstance(addr, str):
            addr = int(addr)
        #salve_value = melsec.execute(1, cst.READ_DISCRETE_INPUTS, addr, bit_len)
        #salve_value = melsec.execute(1, cst.READ_DISCRETE_INPUTS, addr, bit_len)
        #salve_value = melsec.execute(1, cst.WRITE_SINGLE_REGISTER, addr, 123)
        salve_value = melsec.execute(1, cst.READ_HOLDING_REGISTERS, addr, bit_len)
        return salve_value
    except Exception as e:
        print(f'_try_plc_read {str(e)} {addr} bit_len={bit_len}')
        modbus_tcp_reopen()
        return None

def plc_read(addr=0, bit_len=1):
    #print('plc_read', addr)
    for i in range(10):
        value = _try_plc_read(addr, bit_len)
        if value is not None:
            if isinstance(value, tuple):
                if len(value)==1:
                    value = value[0]
            #print(f'plc_read {addr} == {value}')
            return value
        time.sleep(0.1)
    
    return -1

def _try_plc_write(addr, value):
    #if addr>=base_addr:
    #    addr -= base_addr
    try:
        if isinstance(addr, str):
            addr = int(addr)
        if isinstance(value, list):
            salve_value = melsec.execute(1, cst.WRITE_MULTIPLE_REGISTERS, addr, output_value=value)
            return salve_value
        salve_value = melsec.execute(1, cst.WRITE_SINGLE_REGISTER, addr, output_value=value)
        #print("salve_value: ",salve_value)
        return salve_value
    except Exception as e:
        print(f'_try_plc_write {str(e)} {addr} {value}')
        modbus_tcp_reopen()
        return None

def plc_write(addr, value):
    #print('plc_write', addr, value)
    for i in range(10):
        x = _try_plc_write(addr, value)
        if x is not None:
            return x
        time.sleep(0.1)
    
    return -1

def plc_writes(addr, value):
    #print(plc_read(addr, 3))
    return plc_write(addr, value)

def plc_set(addr, a):
    a0 = a&0xffff
    #plc_writes(addr, [a0, a1])
    plc_writes(addr, [a0])
    #print(addr, plc_read(addr, 1))
    return 0


def plc_set16(addr, a):
    a0 = a&0xffff
    # plc_writes(addr, [a0, a1])
    #logger.info(f'plc_set16 {addr} {a}')
    plc_writes(addr, [a0])
    #print(addr, plc_read(addr, 2))
    return 0

import time
def plc_get16(addr):
    a = plc_read(addr, 1)
    #logger.info(f'plc_get16 {addr} {a}')
    # pos = (a[1]<<16)+a[0]
    #a = a[0]
    return a

def plc_set32(addr, a):
    a0 = a&0xffff
    a1 = a>>16
    # plc_writes(addr, [a0, a1])
    plc_writes(addr, [a1, a0])
    #print(addr, plc_read(addr, 2))
    return 0

def plc_get32(addr):
    a = plc_read(addr, 2)
    # pos = (a[1]<<16)+a[0]
    pos = (a[0]<<16)+a[1]
    return pos


pre_live_time = time.time()
plc_live_stage = 0
def plc_live():
    t = time.time()
    global pre_live_time
    global plc_live_stage
    tt = t-pre_live_time
    if tt>4:
        a = 0 if plc_live_stage%2==0 else 1
        plc_live_stage += 1
        plc_set16(700, a)
        pre_live_time = t

# x 252
# y 252
# z 252
def plc_move(pos, i=0):
    v = []
    k = len(pos)
    for x in pos:
        x = int(x)
        hx = x // 65536
        lx = x % 65536
        v += [lx, hx]
    
    plc_writes(730+i*2, v)
    plc_writes(701+i, [1]*k)
    print(plc_read(701+i, 10))
    
    t0 = time.time()
    while 1:
        t1 = time.time()
        tt = t1-t0
        if tt>10:
            return 0
        v = plc_read(611, k)
        if all(v):
            return 1
        time.sleep(0.1)

    return 0

def plc_get_move1(i):
    y = plc_read(701+i, 1)
    return y

def plc_move1(x, i):
    v = []
    k = 1
    x = int(x)
    hx = x // 65536
    lx = x % 65536
    v += [lx, hx]
    
    plc_writes(730+i*2, v)
    plc_writes(701+i, [1]*k)
    #print(plc_read(701+i, 10))
    
    t0 = time.time()
    while 1:
        t1 = time.time()
        tt = t1-t0
        if tt>10:
            return 0
        v = plc_read(611+i, k)
        if all(v):
            return 1
        time.sleep(0.1)

    return 0

def test_move():
    kkk = [0, 20]*100
    for kk in kkk:
        k = kk*100
        plc_move1(k, 3)
        #print(plc_read(701, 10))
        print(k)
        time.sleep(2)

    kkk = [0, 150]*100

    if 0:
        kkk = [0, 150]*100
        for kk in kkk:
            k = kk*1000
            #plc_move1(k, 1)
            #print(plc_read(701, 10))
            print(k)
            time.sleep(1)


    kkk = [0, 150]*100
    for kk in kkk:
        k = kk*1000
        plc_move([k, k, k])
        #print(plc_read(701, 10))
        print(k)
        time.sleep(1)

    print(plc_read(730, 10))

def get_cam_ready1():
    addr = 608
    v = plc_read(addr)
    plc_write(addr, 0)
    return v

def plc_set_ng(end):
    v = plc_write(708, end)
    if end==0:
        v = plc_write(709, 1)
    return v

def plc_get_code():
    return 0

#print(plc_read(619, 10))
def plc_move_action(toend):
    t = 'xbegin'
    if toend:
        t = 'xend'
    pos = [config[t], config[t]]
    plc_move(pos)
    plc_read()
